#include "OgreRefAppSteeringBehaviors.h"
#include "Ogre.h"
#include "OgreVector3.h"
#include "OgreRefAppWorld.h"

#define __DEBUGON__

#include <iostream>
#include <fstream>
using namespace std;

using namespace Ogre;



namespace OgreRefApp
{

    SteeringBehavior::SteeringBehavior(ApplicationObject* agent):


            m_pOwner(agent),
            m_iFlags(0),
            m_SummingMethod(prioritized)


    {


    }
    bool      SteeringBehavior::On(behavior_type bt)
    {
        return (m_iFlags & bt) == bt;
    }
//---------------------------------dtor ----------------------------------
    SteeringBehavior::~SteeringBehavior(){}

    Vector3 SteeringBehavior::Calculate()
    {
        //reset the steering force
        m_vSteeringForce *=0;


        switch (m_SummingMethod)
        {
        case weighted_average:

            //m_vSteeringForce = CalculateWeightedSum(); break;

        case prioritized:
            m_vSteeringForce = CalculatePrioritized();
            break;

        case dithered:

            //m_vSteeringForce = CalculateDithered();break;

        default:
            m_vSteeringForce = Vector3(0,0,0);

        }//end switch

        return m_vSteeringForce;
    }

    bool SteeringBehavior::AccumulateForce(Ogre::Vector3 &RunningTot, Ogre::Vector3 ForceToAdd)
    {
        Real MagnitudeSoFar = RunningTot.length();

        Real MagnitudeRemaining = m_pOwner->getMaxLinearVelocity().length() - MagnitudeSoFar;

        if (MagnitudeRemaining<=0.0) return false;

        Ogre::Vector3 MagnitudeToAdd = ForceToAdd;
            std::cout << "acumulate "<< std::endl;

        if ( MagnitudeToAdd.length() < MagnitudeRemaining )
        {
            RunningTot +=ForceToAdd;
        }
        else
        {
            RunningTot += ( ForceToAdd / Vector3( ForceToAdd ).normalise() ) * MagnitudeRemaining;
        }

        return true;

    }


    Vector3 SteeringBehavior::CalculatePrioritized()
    {

        Vector3 force;

        m_vSteeringForce = m_pOwner->getLinearVelocity();

        if (m_iFlags!=0) m_vSteeringForce*=0;


        if (On(seek))
        {
            force = Seek(m_vTarget);
            if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }

        if (On(arrive))
        {
            force = Arrive(m_vTarget);
            if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }

        if (On(pursuit))
        {
            force = Pursuit(m_pTargetAgent1);
            if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }

        if (On(evade))
        {
            force = Evade(m_pTargetAgent1);
            if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }

        if (On(interpose))
        {

            force = Interpose(m_pTargetAgent1,m_pTargetAgent2);
            if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }
        if (On(separation))
        {
            //force = Separation( World::getSingletonPtr()->getObjects() );
            //if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;

        }

        return m_vSteeringForce;
    }

    Vector3 SteeringBehavior::Seek(Vector3 TargetPos)
    {

        Vector3 DesiredVelocity = m_vTarget - m_pOwner->getPosition() - m_pOwner->getLinearVelocity();

        if ( Vector3( DesiredVelocity ).normalise() > 1 )
            DesiredVelocity = ( DesiredVelocity / Vector3( DesiredVelocity ).normalise() ) * m_pOwner->getMaxLinearVelocity();
        //DesiredVelocity.makeCeil(m_pOwner->getMaxLinearVelocity());

        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (DesiredVelocity*Vector3(1,1,0) + Vector3(0,0,-59));



    }

    Vector3 SteeringBehavior::Arrive(Vector3 TargetPos)
    {

        Vector3 DesiredVelocity = m_vTarget - m_pOwner->getPosition();

        if ( Vector3( DesiredVelocity ).normalise() > 1 )
            DesiredVelocity = ( DesiredVelocity / Vector3( DesiredVelocity ).normalise() ) * m_pOwner->getMaxLinearVelocity();
        //DesiredVelocity.makeCeil(m_pOwner->getMaxLinearVelocity());


        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (DesiredVelocity*Vector3(1,1,0) + Vector3(0,0,-59));




    }

    Vector3 SteeringBehavior::Pursuit(ApplicationObject* agent)
    {
        Vector3 DesiredVelocity = agent->getPosition() + agent->getLinearVelocity() - m_pOwner->getPosition();


        if ( Vector3( DesiredVelocity ).normalise() > 1 )
            DesiredVelocity = ( DesiredVelocity / Vector3( DesiredVelocity ).normalise() ) * m_pOwner->getMaxLinearVelocity();
        //DesiredVelocity.makeCeil(m_pOwner->getMaxLinearVelocity());

        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (DesiredVelocity*Vector3(1,1,0) + Vector3(0,0,-59));



    }

    Vector3 SteeringBehavior::Evade(ApplicationObject* agent)
    {
        Vector3 DesiredVelocity = agent->getPosition() + agent->getLinearVelocity() + m_pOwner->getPosition();


        if ( Vector3( DesiredVelocity ).normalise() > 1 )
            DesiredVelocity = ( DesiredVelocity / Vector3( DesiredVelocity ).normalise() ) * m_pOwner->getMaxLinearVelocity();
        //DesiredVelocity.makeCeil(m_pOwner->getMaxLinearVelocity());

        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (DesiredVelocity*Vector3(1,1,0) + Vector3(0,0,-59));



    }

    Vector3 SteeringBehavior::Interpose(ApplicationObject* agentA, ApplicationObject* agentB)
    {
        /*
        Vector3 DesiredVelocity = ( ( agentA->getPosition() + agentA->getLinearVelocity() + agentB->getPosition() + agentB->getLinearVelocity() ) / 2 ) - m_pOwner->getPosition();

        if ( Vector3( DesiredVelocity ).normalise() > 1 )
            DesiredVelocity = ( DesiredVelocity / Vector3( DesiredVelocity ).normalise() ) * m_pOwner->getMaxLinearVelocity();
        //DesiredVelocity.makeCeil(m_pOwner->getMaxLinearVelocity());

        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (DesiredVelocity*Vector3(1,1,0) + Vector3(0,0,-3));
        */
        return ( Ogre::Vector3(0,0,0) );



    }

    Ogre::Vector3 SteeringBehavior::Separation(std::map<String, ApplicationObject*> agents)
    {


        //return (DesiredVelocity - m_pOwner->getLinearVelocity());
        return (Ogre::Vector3(0,0,0));


    }



}
